// Copyright 2022 Jeroen van Nugteren

// Permission is hereby granted, free of charge, to any person obtaining a copy of this software
// and associated documentation files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all copies or
// substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

// include header
#include "dfellipse.hh"

// code specific to Rat
namespace rat{namespace dm{
	
	// constructor
	DFEllipse::DFEllipse(){
		set_name("Ellipse");
	}

	// constructcor
	DFEllipse::DFEllipse(const fltp a, const fltp b, const fltp xe, const fltp ye) : DFEllipse(){
		a_ = a; b_ = b; xe_ = xe; ye_ = ye;
	}

	// factory
	ShDFEllipsePr DFEllipse::create(){
		return std::make_shared<DFEllipse>();
	}

	// factory
	ShDFEllipsePr DFEllipse::create(const fltp a, const fltp b, const fltp xe, const fltp ye){
		return std::make_shared<DFEllipse>(a,b,xe,ye);
	}

	// setters
	void DFEllipse::set_a(const fltp a){
		a_ = a;
	}

	void DFEllipse::set_b(const fltp b){
		b_ = b;
	}

	void DFEllipse::set_xe(const fltp xe){
		xe_ = xe;
	}

	void DFEllipse::set_ye(const fltp ye){
		ye_ = ye;
	}

	// getters
	fltp DFEllipse::get_a()const{
		return a_;
	}

	fltp DFEllipse::get_b()const{
		return b_;
	}

	fltp DFEllipse::get_xe()const{
		return xe_;
	}

	fltp DFEllipse::get_ye()const{
		return ye_;
	}

	// get bounding box
	arma::Mat<fltp>::fixed<2,2> DFEllipse::get_bounding() const{
		// allocate
		arma::Mat<fltp>::fixed<2,2> Mb;
		
		// bounding in x and y
		Mb.col(0) = arma::Col<fltp>::fixed<2>{xe_-a_,xe_+a_};
		Mb.col(1) = arma::Col<fltp>::fixed<2>{ye_-b_,ye_+b_};

		// return box
		return Mb;
	}

	// distance function
	// functions to solve
	// https://www.ma.imperial.ac.uk/~rn/distance2ellipse.pdf
	// f = (a_*a_ - b_*b_)*r*cos(theta)*sin(theta) - x*a_*sin(theta) + y*b_*cos(theta) = 0
	// df = (a_*a_ - b_*b_)*r*(cos(theta)^2 - sin(theta)^2) - x*a_*cos(theta) - y*b_*sin(theta)
	arma::Col<fltp> DFEllipse::calc_distance(const arma::Mat<fltp> &p) const{
		// settings
		const fltp r = RAT_CONST(1.0);
		const arma::uword max_iter = 10;
		// const fltp abstol = arma::Datum<fltp>::tau/360/10.0;

		// get x and y
		const arma::Col<fltp> x = p.col(0) - xe_;
		const arma::Col<fltp> y = p.col(1) - ye_;

		// initial guesses for theta
		arma::Col<fltp> theta = arma::atan2(a_*y,b_*x);

		// newton method
		arma::uword iter=0;
		for(;iter<max_iter;iter++){
			// evaluate function and derivative
			const arma::Col<fltp> fval = r*(a_*a_ - b_*b_)*
				arma::cos(theta)%arma::sin(theta) - 
				a_*x%arma::sin(theta) + b_*y%arma::cos(theta);
			const arma::Col<fltp> dfdtheta = r*(a_*a_ - b_*b_)*
				(arma::square(arma::cos(theta)) - arma::square(arma::sin(theta))) - 
				a_*x%arma::cos(theta) - b_*y%arma::sin(theta);

			// update theta
			theta -= fval/dfdtheta;

			// stop condition
			// if(arma::all(arma::abs(fval)<abstol))break;
		}

		// check
		// if(iter==max_iter)rat_throw_line("ellipse distance function newton method did not converge");

		// find point on 
		const arma::Col<fltp> xn = r*a_*arma::cos(theta);
		const arma::Col<fltp> yn = r*b_*arma::sin(theta);

		// distance
		const arma::Col<fltp> d = arma::hypot(x - xn, y - yn);

		// calculate sign of distance
		const arma::Col<fltp> sgn = arma::sign(arma::square(x/a_) + arma::square(y/b_) - r*r);

		// return distance 
		return sgn%d;
	}

	// validity check
	bool DFEllipse::is_valid(const bool enable_throws)const{
		// check user input
		if(a_<=0){if(enable_throws){rat_throw_line("a must be larger than zero");} return false;};
		if(b_<=0){if(enable_throws){rat_throw_line("b must be larger than zero");} return false;};
		return true;
	}

	// type string for serialization
	std::string DFEllipse::get_type(){
		return "rat::dm::dfellipse";
	}

	// method for serialization into json
	void DFEllipse::serialize(Json::Value &js, cmn::SList &list) const{
		// parent
		DistFun::serialize(js,list);

		// store type ID
		js["type"] = get_type();
		js["a"] = a_; js["b"] = b_;
		js["xe"] = xe_; js["ye"] = ye_;
	}

	// method for deserialisation from json
	void DFEllipse::deserialize(
		const Json::Value &js, cmn::DSList &list, 
		const cmn::NodeFactoryMap &factory_list, 
		const boost::filesystem::path &pth){

		// parent
		DistFun::deserialize(js,list,factory_list,pth);

		// get values
		a_ = js["a"].ASFLTP(); b_ = js["b"].ASFLTP();
		xe_ = js["xe"].ASFLTP(); ye_ = js["ye"].ASFLTP();
	}

}}